
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sitl_state.c
  * @author     baiyang
  * @date       2022-12-1
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#define ALLOW_DOUBLE_MATH_FUNCTIONS

#include "sitl_state.h"
#include "sim_jsbsim.h"
#include "sim_uart.h"

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>

#include <rtconfig.h>

#include <uITC/uITC.h>
#include <uITC/uITC_msg.h>
#include <common/time/gp_time.h>
#include <vehicle/vehicle_type.h>
#include <common/geo/declination.h>
#include <board_config/borad_config.h>

#if MB_BUILD_TYPE(MICROBEE_BUILD_Copter)
#include <copter/copter.h>
#endif
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void _create_serial_sim(sitl_state_t sitl);
static void _fdm_input_local(sitl_state_t sitl);
static void _simulator_servos(sitl_state_t sitl, struct sitl_input *input);
static void _output_to_flightgear(sitl_state_t sitl);
static void _set_height_agl(sitl_state_t sitl);
static void _state_decode(sitl_state_t sitl);
/*----------------------------------variable----------------------------------*/
static struct sitl_state sitlstate;

static rt_thread_t thread_ctx;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
sitl_state_t sitlstate_get_singleton()
{
    return &sitlstate;
}

void sitlstate_ctor(sitl_state_t sitl)
{
    if (sitl->construction_completed) {
        return;
    }

    sitl->construction_completed = true;
    sitl->output_ready = false;

    sitl->_uart_path[0] = "udpclient:127.0.0.1:14550";
    sitl->_uart_path[1] = "udpclient:127.0.0.1:14551";
    sitl->_uart_path[2] = "udpclient:127.0.0.1:14552";
    sitl->_uart_path[3] = "udpclient:127.0.0.1:14553";
    sitl->_uart_path[4] = "udpclient:127.0.0.1:14554";
    sitl->_uart_path[5] = "udpclient:127.0.0.1:14555";
    sitl->_uart_path[6] = "udpclient:127.0.0.1:14556";
    sitl->_uart_path[7] = "udpclient:127.0.0.1:14557";
    sitl->_uart_path[8] = "udpclient:127.0.0.1:14558";

    mb_socket_ctor(&sitl->_sitl_rc_in, true);
    mb_socket_ctor(&sitl->fg_socket, true);
}

void sitlstate_sitl_setup(sitl_state_t sitl)
{
#if 0
    sitl->_parent_pid = getppid();
#endif

    sitlstate_setup_fdm(sitl);

    _create_serial_sim(sitl);

    brd_init();

#if MB_BUILD_TYPE(MICROBEE_BUILD_Copter)
    copter_main();
#endif

    printf("Starting SITL input\n");

    // find the barometer object if it exists
    sitl->_sitl = sim_get_singleton();

    if (sitl->_sitl != NULL) {
#if 0
        // setup some initial values
        _update_airspeed(0);
        if (sitl->enable_gimbal) {
            gimbal = new SITL::Gimbal(_sitl->state);
        }

        sitl_model->set_buzzer(&_sitl->buzzer_sim);
        sitl_model->set_sprayer(&_sitl->sprayer_sim);
        sitl_model->set_gripper_servo(&_sitl->gripper_sim);
        sitl_model->set_gripper_epm(&_sitl->gripper_epm_sim);
        sitl_model->set_parachute(&_sitl->parachute_sim);
        sitl_model->set_precland(&_sitl->precland_sim);
        _sitl->i2c_sim.init();
        sitl_model->set_i2c(&_sitl->i2c_sim);
#endif
        if (sitl->_use_fg_view) {
            mb_socket_connect(&sitl->fg_socket , sitl->_fg_address, sitl->_fg_view_port);
        }

        printf("Using Irlock at port : %d\n", sitl->_irlock_port);
        sitl->_sitl->irlock_port = sitl->_irlock_port;
    }

#if 0
    if (sitl->_synthetic_clock_mode) {
        // start with non-zero clock
        hal.scheduler->stop_clock(1);
    }
#endif
}

/*
  setup a SITL FDM listening UDP port
 */
void sitlstate_setup_fdm(sitl_state_t sitl)
{
#if 0
    if (!_sitl_rc_in.reuseaddress()) {
        fprintf(stderr, "SITL: socket reuseaddress failed on RC in port: %d - %s\n", _rcin_port, strerror(errno));
        fprintf(stderr, "Aborting launch...\n");
        exit(1);
    }
    if (!_sitl_rc_in.bind("0.0.0.0", _rcin_port)) {
        fprintf(stderr, "SITL: socket bind failed on RC in port : %d - %s\n", _rcin_port, strerror(errno));
        fprintf(stderr, "Aborting launch...\n");
        exit(1);
    }
    if (!_sitl_rc_in.set_blocking(false)) {
        fprintf(stderr, "SITL: socket set_blocking(false) failed on RC in port: %d - %s\n", _rcin_port, strerror(errno));
        fprintf(stderr, "Aborting launch...\n");
        exit(1);
    }
    if (!_sitl_rc_in.set_cloexec()) {
        fprintf(stderr, "SITL: socket set_cloexec() failed on RC in port: %d - %s\n", _rcin_port, strerror(errno));
        fprintf(stderr, "Aborting launch...\n");
        exit(1);
    }
#endif
}

/*
  step the FDM by one time step
 */
void sitlstate_fdm_input_step(sitl_state_t sitl)
{
    static uint32_t last_pwm_input = 0;

    _fdm_input_local(sitl);

    // simulate RC input at 50Hz
    if (time_millis() - last_pwm_input >= 20 && sitl->_sitl != NULL && sitl->_sitl->rc_fail != SITL_RCFail_NoPulses) {
        last_pwm_input = time_millis();
        sitl->new_rc_input = true;
    }

#if 0
    _scheduler->sitl_begin_atomic();

    if (sitl->_update_count == 0 && _sitl != NULL) {
        HALSITL::Scheduler::timer_event();
        _scheduler->sitl_end_atomic();
        return;
    }

    if (sitl->_sitl != NULL) {
        _update_airspeed(_sitl->state.airspeed);
        _update_rangefinder();
    }
#endif
}

#define streq(a, b) (!strcmp(a, b))
static void _create_serial_sim(sitl_state_t sitl)
{
    static struct sim_uart simuart[ARRAY_SIZE(sitl->_uart_path)] = {0};
#if 0
    for (uint8_t i=0; i<ARRAY_SIZE(simuart); i++) {
        char uart_name[9] = {0};
        snprintf(uart_name, sizeof(uart_name), "uart%d", i);
        sim_uart_init(&simuart[i], uart_name, sitl->_uart_path[i], sitl->_base_port, sitl->_udp_port_offset);
    }
#endif

    sim_uart_init(&simuart[0], "uart0", sitl->_uart_path[0], sitl->_base_port, sitl->_udp_port_offset);
}

/*
  get FDM input from a local model
 */
static void _fdm_input_local(sitl_state_t sitl)
{
    struct sitl_input input;

#if 0
    // check for direct RC input
    if (sitl->_sitl != NULL) {
        _check_rc_input();
    }
#endif

    // construct servos structure for FDM
    _simulator_servos(sitl, &input);

    // update the model
    sim_aircraft_update_model(sitl->sitl_model, &input);

    // get FDM output from the model
    if (sitl->_sitl) {
        sim_aircraft_fill_fdm(sitl->sitl_model, &sitl->_sitl->state);

        if (sitl->_sitl->rc_fail == SITL_RCFail_None) {
            for (uint8_t i=0; i< sitl->_sitl->state.rcin_chan_count; i++) {
                sitl->pwm_input[i] = 1000 + (uint16_t)(sitl->_sitl->state.rcin[i]*1000);
            }
        }
    }

#if 0
#if HAL_SIM_JSON_MASTER_ENABLED
    // output JSON state to ride along flight controllers
    ride_along.send(_sitl->state,sitl_model->get_position_relhome());
#endif

    if (gimbal != nullptr) {
        gimbal->update();
    }
#if HAL_SIM_ADSB_ENABLED
    if (adsb != nullptr) {
        adsb->update(*sitl_model);
    }
#endif
    if (vicon != nullptr) {
        Quaternion attitude;
        sitl_model->get_attitude(attitude);
        vicon->update(sitl_model->get_location(),
                      sitl_model->get_position_relhome(),
                      sitl_model->get_velocity_ef(),
                      attitude);
    }
    if (benewake_tf02 != nullptr) {
        benewake_tf02->update(sitl_model->rangefinder_range());
    }
    if (benewake_tf03 != nullptr) {
        benewake_tf03->update(sitl_model->rangefinder_range());
    }
    if (benewake_tfmini != nullptr) {
        benewake_tfmini->update(sitl_model->rangefinder_range());
    }
    if (teraranger_serial != nullptr) {
        teraranger_serial->update(sitl_model->rangefinder_range());
    }
    if (lightwareserial != nullptr) {
        lightwareserial->update(sitl_model->rangefinder_range());
    }
    if (lightwareserial_binary != nullptr) {
        lightwareserial_binary->update(sitl_model->rangefinder_range());
    }
    if (lanbao != nullptr) {
        lanbao->update(sitl_model->rangefinder_range());
    }
    if (blping != nullptr) {
        blping->update(sitl_model->rangefinder_range());
    }
    if (leddarone != nullptr) {
        leddarone->update(sitl_model->rangefinder_range());
    }
    if (USD1_v0 != nullptr) {
        USD1_v0->update(sitl_model->rangefinder_range());
    }
    if (USD1_v1 != nullptr) {
        USD1_v1->update(sitl_model->rangefinder_range());
    }
    if (maxsonarseriallv != nullptr) {
        maxsonarseriallv->update(sitl_model->rangefinder_range());
    }
    if (wasp != nullptr) {
        wasp->update(sitl_model->rangefinder_range());
    }
    if (nmea != nullptr) {
        nmea->update(sitl_model->rangefinder_range());
    }
    if (rf_mavlink != nullptr) {
        rf_mavlink->update(sitl_model->rangefinder_range());
    }
    if (gyus42v2 != nullptr) {
        gyus42v2->update(sitl_model->rangefinder_range());
    }
    if (efi_ms != nullptr) {
        efi_ms->update();
    }

    if (frsky_d != nullptr) {
        frsky_d->update();
    }
    // if (frsky_sport != nullptr) {
    //     frsky_sport->update();
    // }
    // if (frsky_sportpassthrough != nullptr) {
    //     frsky_sportpassthrough->update();
    // }

#if AP_SIM_CRSF_ENABLED
    if (crsf != nullptr) {
        crsf->update();
    }
#endif

#if HAL_SIM_PS_RPLIDARA2_ENABLED
    if (rplidara2 != nullptr) {
        rplidara2->update(sitl_model->get_location());
    }
#endif

#if HAL_SIM_PS_TERARANGERTOWER_ENABLED
    if (terarangertower != nullptr) {
        terarangertower->update(sitl_model->get_location());
    }
#endif

#if HAL_SIM_PS_LIGHTWARE_SF45B_ENABLED
    if (sf45b != nullptr) {
        sf45b->update(sitl_model->get_location());
    }
#endif

    if (vectornav != nullptr) {
        vectornav->update();
    }

    if (lord != nullptr) {
        lord->update();
    }

#if HAL_SIM_AIS_ENABLED
    if (ais != nullptr) {
        ais->update();
    }
#endif
    for (uint8_t i=0; i<ARRAY_SIZE(gps); i++) {
        if (gps[i] != nullptr) {
            gps[i]->update();
        }
    }
#endif

    DEFINE_TIMETAG(output_to_flightgear, 30);
    if (sitl->_sitl && sitl->_use_fg_view && time_check_tag(TIMETAG(output_to_flightgear))) {
        _output_to_flightgear(sitl);
    }

#if 0
    // update simulation time
    if (sitl->_sitl) {
        hal.scheduler->stop_clock(_sitl->state.timestamp_us);
    } else {
        hal.scheduler->stop_clock(AP_HAL::micros64()+100);
    }
#endif

    _set_height_agl(sitl);

    _state_decode(sitl);

    sitl->_synthetic_clock_mode = true;
    sitl->_update_count++;
}

// return number of bits available
static uint8_t _ffs(uint32_t bits)
{
    uint8_t bit = 0;

    if (0 == bits)
        return 0;

    for (bit = 1; !(bits & 1); ++bit) {
        bits >>= 1;
    }

    return bit;
}

/*
  create sitl_input structure for sending to FDM
 */
static void _simulator_servos(sitl_state_t sitl, struct sitl_input *input)
{
    static uint32_t last_update_usec = 0;

    /* this maps the registers used for PWM outputs. The RC
     * driver updates these whenever it wants the channel output
     * to change */

    if (last_update_usec == 0 || !sitl->output_ready) {
        for (uint8_t i=0; i<SITL_NUM_CHANNELS; i++) {
            sitl->pwm_output[i] = 1000;
        }
        if (sitl->_vehicle == SIM_Plane) {
            sitl->pwm_output[0] = sitl->pwm_output[1] = sitl->pwm_output[3] = 1500;
        }
        if (sitl->_vehicle == SIM_Rover) {
            sitl->pwm_output[0] = sitl->pwm_output[1] = sitl->pwm_output[2] = sitl->pwm_output[3] = 1500;
        }
        if (sitl->_vehicle == SIM_Sub) {
            sitl->pwm_output[0] = sitl->pwm_output[1] = sitl->pwm_output[2] = sitl->pwm_output[3] =
                    sitl->pwm_output[4] = sitl->pwm_output[5] = sitl->pwm_output[6] = sitl->pwm_output[7] = 1500;
        }
    }

    uitc_actuator_controls actuator_controls = {0};
    if (itc_copy_from_hub(ITC_ID(vehicle_actuator_controls), &actuator_controls) == 0) {
        sitl->output_ready = true;

        for (uint8_t i=0; i<NUM_ACTUATOR_OUTPUTS; i++) {
            sitl->pwm_output[i] = 1000 + (uint16_t)(actuator_controls.control[i] * 1000);
        }
    }

    // output at chosen framerate
    uint32_t now = time_micros();
    last_update_usec = now;

    float altitude = sim_aircraft_hagl(sitl->sitl_model);
    float wind_speed = 0;
    float wind_direction = 0;
    float wind_dir_z = 0;

    // give 5 seconds to calibrate airspeed sensor at 0 wind speed
    if (sitl->wind_start_delay_micros == 0) {
        sitl->wind_start_delay_micros = now;
    } else if (sitl->_sitl && (now - sitl->wind_start_delay_micros) > 5000000 ) {
        // The EKF does not like step inputs so this LPF keeps it happy.
        wind_speed =     sitl->_sitl->wind_speed_active     = (0.95f*sitl->_sitl->wind_speed_active)     + (0.05f*sitl->_sitl->wind_speed);
        wind_direction = sitl->_sitl->wind_direction_active = (0.95f*sitl->_sitl->wind_direction_active) + (0.05f*sitl->_sitl->wind_direction);
        wind_dir_z =     sitl->_sitl->wind_dir_z_active     = (0.95f*sitl->_sitl->wind_dir_z_active)     + (0.05f*sitl->_sitl->wind_dir_z);
        
        // pass wind into simulators using different wind types via param SIM_WIND_T*.
        switch (sitl->_sitl->wind_type) {
        case WIND_TYPE_SQRT:
            if (altitude < sitl->_sitl->wind_type_alt) {
                wind_speed *= sqrtf(MAX(altitude / sitl->_sitl->wind_type_alt, 0));
            }
            break;

        case WIND_TYPE_COEF:
            wind_speed += (altitude - sitl->_sitl->wind_type_alt) * sitl->_sitl->wind_type_coef;
            break;

        case WIND_TYPE_NO_LIMIT:
        default:
            break;
        }

        // never allow negative wind velocity
        wind_speed = MAX(wind_speed, 0);
    }

    input->wind.speed = wind_speed;
    input->wind.direction = wind_direction;
    input->wind.turbulence = sitl->_sitl?sitl->_sitl->wind_turbulance:0;
    input->wind.dir_z = wind_dir_z;

    for (uint8_t i=0; i<SITL_NUM_CHANNELS; i++) {
        if (sitl->pwm_output[i] == 0xFFFF) {
            input->servos[i] = 0;
        } else {
            input->servos[i] = sitl->pwm_output[i];
        }
    }

#if 0
    if (sitl->_sitl != NULL) {
        // FETtec ESC simulation support.  Input signals of 1000-2000
        // are positive thrust, 0 to 1000 are negative thrust.  Deeper
        // changes required to support negative thrust - potentially
        // adding a field to input.
        if (sitl->_sitl != NULL) {
            if (_sitl->fetteconewireesc_sim.enabled()) {
                _sitl->fetteconewireesc_sim.update_sitl_input_pwm(input);
                for (uint8_t i=0; i<ARRAY_SIZE(input.servos); i++) {
                    if (input.servos[i] != 0 && input.servos[i] < 1000) {
                        AP_HAL::panic("Bad input servo value (%u)", input.servos[i]);
                    }
                }
            }
        }
    }
#endif

    float engine_mul = sitl->_sitl?sitl->_sitl->engine_mul:1;
    uint8_t engine_fail = sitl->_sitl?sitl->_sitl->engine_fail:0;
    float throttle = 0.0f;
    
    if (engine_fail >= ARRAY_SIZE(input->servos)) {
        engine_fail = 0;
    }
    // apply engine multiplier to motor defined by the SIM_ENGINE_FAIL parameter
    if (sitl->_vehicle != SIM_Rover) {
        input->servos[engine_fail] = (uint16_t)((input->servos[engine_fail]-1000) * engine_mul) + 1000;
    } else {
        input->servos[engine_fail] = (uint16_t)((input->servos[engine_fail] - 1500) * engine_mul) + 1500;
    }

    if (sitl->_vehicle == SIM_Plane) {
        float forward_throttle = math_constrain_float((input->servos[2] - 1000) / 1000.0f, 0.0f, 1.0f);
        // do a little quadplane dance
        float hover_throttle = 0.0f;
        uint8_t running_motors = 0;
        uint32_t mask = sitl->_sitl->state.motor_mask;
        uint8_t bit;
        while ((bit = _ffs(mask)) != 0) {
            uint8_t motor = bit-1;
            mask &= ~(1U<<motor);
            float motor_throttle = math_constrain_float((input->servos[motor] - 1000) / 1000.0f, 0.0f, 1.0f);
            // update motor_on flag
            if (!math_flt_zero(motor_throttle)) {
                hover_throttle += motor_throttle;
                running_motors++;
            }
        }
        if (running_motors > 0) {
            hover_throttle /= running_motors;
        }
        if (!math_flt_zero(forward_throttle)) {
            throttle = forward_throttle;
        } else {
            throttle = hover_throttle;
        }
    } else if (sitl->_vehicle == SIM_Rover) {
        input->servos[2] = (uint16_t)(math_constrain_int16(input->servos[2], 1000, 2000));
        input->servos[0] = (uint16_t)(math_constrain_int16(input->servos[0], 1000, 2000));
        throttle = fabsf((input->servos[2] - 1500) / 500.0f);
    } else {
        // run checks on each motor
        uint8_t running_motors = 0;
        uint32_t mask = sitl->_sitl->state.motor_mask;
        uint8_t bit;
        while ((bit = _ffs(mask)) != 0) {
            const uint8_t motor = bit-1;
            mask &= ~(1U<<motor);
            float motor_throttle = math_constrain_float((input->servos[motor] - 1000) / 1000.0f, 0.0f, 1.0f);
            // update motor_on flag
            if (!math_flt_zero(motor_throttle)) {
                throttle += motor_throttle;
                running_motors++;
            }
        }
        if (running_motors > 0) {
            throttle /= running_motors;
        }
    }
    if (sitl->_sitl) {
        sitl->_sitl->throttle = throttle;
    }

    float voltage = 0;
    sitl->_current = 0;
    
    if (sitl->_sitl != NULL) {
        if (sitl->_sitl->state.battery_voltage <= 0) {
            if (sitl->_vehicle == SIM_Sub) {
                voltage = sitl->_sitl->batt_voltage;
                for (uint8_t i=0; i<6; i++) {
                    float pwm = input->servos[i];
                    //printf("i: %d, pwm: %.2f\n", i, pwm);
                    float fraction = fabsf((pwm - 1500) / 500.0f);

                    voltage -= fraction * 0.5f;

                    float draw = fraction * 15;
                    sitl->_current += draw;
                }
            } else {
                // simulate simple battery setup
                // lose 0.7V at full throttle
                voltage = sitl->_sitl->batt_voltage - 0.7f * throttle;

                // assume 50A at full throttle
                sitl->_current = 50.0f * throttle;
            }
        } else {
            // FDM provides voltage and current
            voltage = sitl->_sitl->state.battery_voltage;
            sitl->_current = sitl->_sitl->state.battery_current;
        }
    }

    // assume 3DR power brick
    sitl->voltage_pin_value = math_float_to_uint16(((voltage / 10.1f) / 5.0f) * 1024);
    sitl->current_pin_value = math_float_to_uint16(((sitl->_current / 17.0f) / 5.0f) * 1024);
    // fake battery2 as just a 25% gain on the first one
    sitl->voltage2_pin_value = math_float_to_uint16(((voltage * 0.25f / 10.1f) / 5.0f) * 1024);
    sitl->current2_pin_value = math_float_to_uint16(((sitl->_current * 0.25f / 17.0f) / 5.0f) * 1024);
}

/*
  output current state to flightgear
 */
static void _output_to_flightgear(sitl_state_t sitl)
{
    struct FGNetFDM fdm = {0};
    const struct sitl_fdm *sfdm = &sitl->_sitl->state;

    fdm.version = 0x18;
    fdm.padding = 0;
    fdm.longitude = DEG_TO_RAD_DOUBLE*sfdm->longitude;
    fdm.latitude = DEG_TO_RAD_DOUBLE*sfdm->latitude;
    fdm.altitude = sfdm->altitude;
    fdm.agl = (float)sfdm->altitude;
    fdm.phi   = radians((float)sfdm->rollDeg);
    fdm.theta = radians((float)sfdm->pitchDeg);
    fdm.psi   = radians((float)sfdm->yawDeg);
    if (sitl->_vehicle == SIM_Copter) {
        fdm.num_engines = 4;
        for (uint8_t i=0; i<4; i++) {
            fdm.rpm[i] = math_constrain_float((float)(sitl->pwm_output[i]-1000), 0, 1000);
        }
    } else {
        fdm.num_engines = 4;
        fdm.rpm[0] = math_constrain_float((float)(sitl->pwm_output[2]-1000)*3, 0, 3000);
        // for quadplane
        fdm.rpm[1] = math_constrain_float((float)(sitl->pwm_output[5]-1000)*12, 0, 12000);
        fdm.rpm[2] = math_constrain_float((float)(sitl->pwm_output[6]-1000)*12, 0, 12000);
        fdm.rpm[3] = math_constrain_float((float)(sitl->pwm_output[7]-1000)*12, 0, 12000);
    }
    FGNetFDM_ByteSwap(&fdm);

    mb_socket_send(&sitl->fg_socket, &fdm, sizeof(fdm));
}

/*
  set height above the ground in meters
 */
static void _set_height_agl(sitl_state_t sitl)
{
    static float home_alt = -1;

    if (!sitl->_sitl) {
        // in example program
        return;
    }

    if (math_flt_equal(home_alt, -1.0f) && sitl->_sitl->state.altitude > 0) {
        // remember home altitude as first non-zero altitude
        home_alt = (float)sitl->_sitl->state.altitude;
    }

#if 0
#if AP_TERRAIN_AVAILABLE
    if (_sitl != nullptr &&
        _sitl->terrain_enable) {
        // get height above terrain from AP_Terrain. This assumes
        // AP_Terrain is working
        float terrain_height_amsl;
        struct Location location;
        location.lat = _sitl->state.latitude*1.0e7;
        location.lng = _sitl->state.longitude*1.0e7;

        AP_Terrain *_terrain = AP_Terrain::get_singleton();
        if (_terrain != nullptr &&
            _terrain->height_amsl(location, terrain_height_amsl, false)) {
            _sitl->height_agl = _sitl->state.altitude - terrain_height_amsl;
            return;
        }
    }
#endif
#endif

    if (sitl->_sitl != NULL) {
        // fall back to flat earth model
        sitl->_sitl->height_agl = (float)(sitl->_sitl->state.altitude - home_alt);
    }
}

/**
  * @brief       
  * @param[in]     
  * @param[out]  
  * @retval      
  * @note        
  */
static void _state_decode(sitl_state_t sitl)
{
    static uitc_vehicle_origin vehicle_origin = {0};

    uint8_t res = false;
    DEFINE_TIMETAG(baro_update, 20);

    uint64_t now_us = time_micros64();

    uitc_vehicle_hil_state hil_state;
    if (itc_copy_from_hub(ITC_ID(vehicle_hil_state), &hil_state) != 0 && !sitl->sitl_model->home_is_set) {
        return;
    }

    if (!vehicle_origin.valid_alt) {
        vehicle_origin.timestamp_us = time_micros64();
        vehicle_origin.alt = sitl->sitl_model->home.alt * 1e-2;
        vehicle_origin.z   = 0.0f;
        vehicle_origin.valid_alt = true;
        itc_publish(ITC_ID(vehicle_origin), &vehicle_origin);
    }

    if (!vehicle_origin.valid_hpos) {
        vehicle_origin.timestamp_us = now_us;
        vehicle_origin.lat = sitl->sitl_model->home.lat;
        vehicle_origin.lon = sitl->sitl_model->home.lng;

        vehicle_origin.x = 0.0f;
        vehicle_origin.y = 0.0f;

        vehicle_origin.yaw = radians(sitl->sitl_model->home_yaw);
        vehicle_origin.mag_decl = geo_get_declination(vehicle_origin.lat*1e-7, vehicle_origin.lon*1e-7);
        vehicle_origin.valid_hpos = true;

        itc_publish(ITC_ID(vehicle_origin), &vehicle_origin);
    }

    if (!vehicle_origin.valid_lpos && vehicle_origin.valid_hpos && vehicle_origin.valid_alt) {
        vehicle_origin.valid_lpos = true;
        itc_publish(ITC_ID(vehicle_origin), &vehicle_origin);
    }

    uitc_vehicle_attitude vehicle_attitude;
    vehicle_attitude.timestamp_us = now_us;
    vehicle_attitude.vehicle_euler.roll = hil_state.roll;
    vehicle_attitude.vehicle_euler.pitch = hil_state.pitch;
    vehicle_attitude.vehicle_euler.yaw = hil_state.yaw;

    quat_from_euler(&vehicle_attitude.vehicle_quat, vehicle_attitude.vehicle_euler.roll,
                    vehicle_attitude.vehicle_euler.pitch, vehicle_attitude.vehicle_euler.yaw);
    quat_norm(&vehicle_attitude.vehicle_quat);
    
    itc_publish(ITC_ID(vehicle_attitude), &vehicle_attitude);

    uitc_sensor_gyr sensor_gyr;
    sensor_gyr.timestamp_us = now_us;
    sensor_gyr.sensor_gyr_measure[0] = hil_state.rollspeed;
    sensor_gyr.sensor_gyr_measure[1] = hil_state.pitchspeed;
    sensor_gyr.sensor_gyr_measure[2] = hil_state.yawspeed;

    sensor_gyr.sensor_gyr_correct[0] = hil_state.rollspeed;
    sensor_gyr.sensor_gyr_correct[1] = hil_state.pitchspeed;
    sensor_gyr.sensor_gyr_correct[2] = hil_state.yawspeed;

    sensor_gyr.sensor_gyr_filter[0] = hil_state.rollspeed;
    sensor_gyr.sensor_gyr_filter[1] = hil_state.pitchspeed;
    sensor_gyr.sensor_gyr_filter[2] = hil_state.yawspeed;

    itc_publish(ITC_ID(sensor_gyr), &sensor_gyr);

    Vector3f_t acc_n = { 0 };
    Vector3f_t acc_b = { 0 };
    Vector3f_t acc_bf = { 0 };

    acc_bf.x = (float)hil_state.xacc * 0.001f * GRAVITY_MSS;
    acc_bf.y = (float)hil_state.yacc * 0.001f * GRAVITY_MSS;
    acc_bf.z = (float)hil_state.zacc * 0.001f * GRAVITY_MSS;

    quat_vec_rotate(&vehicle_attitude.vehicle_quat, &acc_n, &acc_bf);

    // 模拟加速度计数据，添加重力加速度
    acc_n.z -= GRAVITY_MSS;
    quat_inv_vec_rotate(&vehicle_attitude.vehicle_quat, &acc_b, &acc_n);

    uitc_sensor_acc sensor_acc;
    sensor_acc.timestamp_us = now_us;
    sensor_acc.sensor_acc_measure[0] = acc_b.vec3[0];
    sensor_acc.sensor_acc_measure[1] = acc_b.vec3[1];
    sensor_acc.sensor_acc_measure[2] = acc_b.vec3[2];

    sensor_acc.sensor_acc_correct[0] = acc_b.vec3[0];
    sensor_acc.sensor_acc_correct[1] = acc_b.vec3[1];
    sensor_acc.sensor_acc_correct[2] = acc_b.vec3[2];

    sensor_acc.sensor_acc_filter[0] = acc_b.vec3[0];
    sensor_acc.sensor_acc_filter[1] = acc_b.vec3[1];
    sensor_acc.sensor_acc_filter[2] = acc_b.vec3[2];
    
    itc_publish(ITC_ID(sensor_acc), &sensor_acc);

    Euler_t e_tmp;
    Quat_t att_q_tmp = { 0 };
    e_tmp.pitch = 0.0f;
    e_tmp.roll = 0.0f;
    e_tmp.yaw = Deg2Rad(vehicle_origin.mag_decl);

    quat_from_euler(&att_q_tmp, e_tmp.roll, e_tmp.pitch, e_tmp.yaw);
    quat_norm(&att_q_tmp);

    Vector3f_t mag = { 0 };
    Vector3f_t mag_vec = { 0 };
    Vector3f_t mag_vec_tmp = { 0 };
    uitc_sensor_mag sensor_mag;
    if (vehicle_origin.valid_lpos) {
        mag.x = 0.500f;
        mag.y = 0.0f;
        mag.z = 0.0f;
        quat_vec_rotate(&att_q_tmp, &mag_vec_tmp, &mag);
        quat_inv_vec_rotate(&vehicle_attitude.vehicle_quat, &mag_vec, &mag_vec_tmp);
        sensor_mag.timestamp_us = now_us;

        sensor_mag.sensor_mag_measure[0] = mag_vec.x;
        sensor_mag.sensor_mag_measure[1] = mag_vec.y;
        sensor_mag.sensor_mag_measure[2] = mag_vec.z;

        sensor_mag.sensor_mag_correct[0] = mag_vec.x;
        sensor_mag.sensor_mag_correct[1] = mag_vec.y;
        sensor_mag.sensor_mag_correct[2] = mag_vec.z;
        
        itc_publish(ITC_ID(sensor_mag), &sensor_mag);
    }

    uitc_vehicle_alt alt_info;
    alt_info.timestamp_us = now_us;
    alt_info.alt = (float)(hil_state.alt * 0.001f);
    alt_info.relative_alt = (float)(hil_state.z * 0.01f);
    alt_info.relative_origin = (float)(hil_state.z * 0.01f);
    alt_info.vz = -(float)(hil_state.vz * 0.01f);
    alt_info.az = -(acc_n.z + GRAVITY_MSS);
    alt_info.az_bias = 0;
    itc_publish(ITC_ID(vehicle_alt), &alt_info);

    if (time_check_tag(TIMETAG(baro_update))) {
        uitc_sensor_baro sensor_baro = {0};
        sensor_baro.timestamp_us = now_us;
        sensor_baro.primary = 0;
        sensor_baro.num_instances = 1;
        sensor_baro.altitude_m = alt_info.alt;    //MSL
        sensor_baro.velocity_ms = alt_info.vz;

        /* publish baro data */
        itc_publish(ITC_ID(sensor_baro), &sensor_baro);
    }

    float fSinLat;
    float fCosLat;
    float Rmh;        //补偿半径
    float Rnh;
    float ff = 0.0033528132f;
    fSinLat = sinf(Deg2Rad(vehicle_origin.lat*1e-7));
    fCosLat = cosf(Deg2Rad(vehicle_origin.lat*1e-7));

    Rmh = (6378137 * (1.0f - 2.0f * ff + 3.0f * ff * fSinLat * fSinLat) + alt_info.alt);    //经高度和维度补偿后的半径
    Rnh = (6378137 * (1.0f + ff * fSinLat * fSinLat) + alt_info.alt);

    uitc_vehicle_position pos_info;
    pos_info.timestamp_us = now_us;
    pos_info.lat = hil_state.lat;
    pos_info.lon = hil_state.lon;
    pos_info.x = Deg2Rad((hil_state.lat*1e-7) - vehicle_origin.lat*1e-7) * Rmh;
    pos_info.y = Deg2Rad((hil_state.lon*1e-7) - vehicle_origin.lon*1e-7) * Rnh * fCosLat;
    pos_info.vx = (float)hil_state.vx * 0.01f;
    pos_info.vy = (float)hil_state.vy * 0.01f;
    pos_info.ax = acc_n.x;
    pos_info.ay = acc_n.y;
    pos_info.ax_bias = 0;    // TODO, no estimation bias for ax and ay
    pos_info.ay_bias = 0;
    itc_publish(ITC_ID(vehicle_position), &pos_info);

    TIMETAG_CHECK_EXECUTE(sensor_gps,100,{
        uitc_sensor_gps gps_pos_t;
        gps_pos_t.timestamp_us = now_us;
        gps_pos_t.time_gps_usec = now_us;
        gps_pos_t.lat = hil_state.lat;
        gps_pos_t.lon = hil_state.lon;
        gps_pos_t.alt_msl = alt_info.alt * 1000;
        gps_pos_t.hdop = 1.5f;
        gps_pos_t.vdop = 2.0f;
        gps_pos_t.horizontal_accuracy = 1.5f;
        gps_pos_t.vertical_accuracy = 2.0f;
        gps_pos_t.vel_m_s = sqrtf(pos_info.vx * pos_info.vx
                  + pos_info.vy * pos_info.vy + alt_info.vz * alt_info.vz);
        gps_pos_t.vel_n_m_s = pos_info.vx;
        gps_pos_t.vel_e_m_s = pos_info.vy;
        gps_pos_t.vel_d_m_s = -alt_info.vz;
        gps_pos_t.vel_ned_valid = 1;
        gps_pos_t.fix_type = uITC_GPS_OK_FIX_3D;
        gps_pos_t.num_sats = 18;
        itc_publish(ITC_ID(sensor_gps), &gps_pos_t);}
    )
}

void sitlstate_step_entry(void* parameter)
{
    sitl_state_t sitl = (sitl_state_t)parameter;
    int32_t interval_ms = 1000 / sitl->_sitl->loop_rate_hz;

    while (1) {
        sitlstate_fdm_input_step(sitl);
        rt_thread_mdelay(interval_ms);
    }
}

void sitlstate_init(sitl_state_t sitl, int argc, char * const argv[])
{
    static bool initialized = false;

    if (!initialized) {
        sitlstate_ctor(sitl);

        sitl->pwm_input[0] = sitl->pwm_input[1] = sitl->pwm_input[3] = 1500;
        sitl->pwm_input[4] = sitl->pwm_input[7] = 1800;
        sitl->pwm_input[2] = sitl->pwm_input[5] = sitl->pwm_input[6] = 1000;
    }

    if (sitlstate_parse_command_line(sitl, argc, argv) && thread_ctx == NULL) {
        thread_ctx = rt_thread_create("sitlstate", sitlstate_step_entry, (void *)sitl, 1024*10, 1, 5);

        if (thread_ctx != NULL) {
            rt_thread_startup(thread_ctx);
        }
    }

    initialized = true;
}

#ifdef RT_USING_FINSH
#include <finsh.h>
int sitlstate_start(int argc, char **argv)
{
    sitlstate_init(sitlstate_get_singleton(), argc, argv);
    return 0;
}
MSH_CMD_EXPORT_ALIAS(sitlstate_start, sim_vehicle, start a simulated vehicle.);
#endif
/*------------------------------------test------------------------------------*/


